function [K, B, z, S, trB] = davqquadmat(vM, vF, a)
%DAVQQUADMAT 计算Davenport'q方法中的二次型矩阵K
%
% References:
% [1] 应用导航技术 “多矢量定姿算法”

n = size(vM, 2);
if nargin < 3
    a = 1/n * ones(1, n);
end

B = zeros(3, 3);
for k = 1:n
    B = B + a(1, k)*vM(:, k)*vF(:, k)';
end
z = outer2cross(B)';
S = B' + B;
trB = trace(B);
K = [trB, z'; z, S-trB*eye(3)];
end